// Various function calls for the MikroQuad
void xmitData(){
  digitalWrite(LEDPIN, HIGH);
  Serial.print(',');
  Serial.print((int)(dt*1000)); 
  Serial.print(','); 
  Serial.print(throttleMotorCommand);
  Serial.print(',');
  Serial.print(rollMotorCommand);
  Serial.print(',');
  Serial.print(pitchMotorCommand);
  Serial.print(',');
  Serial.print(yawMotorCommand);
  Serial.print(',');
  Serial.print(frontCommand);
  Serial.print(',');
  Serial.print(leftCommand);
  Serial.print(',');
  Serial.print(rightCommand);
  Serial.print(',');
  Serial.print(rearCommand);
  Serial.print(',');
  Serial.print((int)(accelero_x()*1000));
  Serial.print(',');
  Serial.print((int)(accelero_y()*1000));
  Serial.print(',');
  Serial.print((int)(accelero_z()*1000));
  Serial.print(',');
  Serial.print((int)(roll_angle*1000));
  Serial.print(',');
  Serial.println((int)(pitch_angle*1000));
  digitalWrite(LEDPIN, LOW);  
}

float findZero(int channel) {
  float zero = 0;
  for (int i=0; i< 100; i++) zero += analogRead(channel);
  return (zero / 100.0);
}  

// roll is arctan(az/ay)-PI/2 or arctan2(az, ay)-PI/2
float PredictAccG_roll(float a_y, float a_z) {
  return (atan2(a_y, a_z));
}

// pitch is arctan(az/ax)-PI/2 or arctan2(az, ax)-PI/2
float PredictAccG_pitch(float a_x, float a_z) {
  return (atan2(-a_x, a_z));
}

float readSerial() {
  int bytesAvailable, i;
  char data[128] = "";
  
  while (Serial.available() == 0) {
    delay(100);
  }
  bytesAvailable = Serial.available();
  for (i = 0; i < bytesAvailable; i++) {
    data[i] = Serial.read();
  }
  return strtol(data, NULL, 10)/10.0;
}

void readTransmitter() {
  if (((xmitCheck += 1) % 5) == 0) {
    roll = pulseIn(ROLLPIN, HIGH, SEARCHTIMEOUT);
    aux = pulseIn(AUXPIN, HIGH, SEARCHTIMEOUT);
    pitch = pulseIn(PITCHPIN, HIGH, SEARCHTIMEOUT);
    yaw = pulseIn(YAWPIN, HIGH, SEARCHTIMEOUT);
    throttle = pulseIn(THROTTLEPIN, HIGH, SEARCHTIMEOUT); //why does throttle and aux read the same?
    gear = pulseIn(GEARPIN, HIGH, SEARCHTIMEOUT);
  }
  if ((xmitCheck % 5) == 2) xmitData();
}  
